#include "ViewFrustum.hpp"
#include "../Collisions/BoundingSphere.hpp"
#include "../Collisions/BoundingBox.hpp"
#include <math.h>

void CullingPlane::setCenter(MVector3D& center)
{
	m_center = center;
}

void CullingPlane::setNormal(MVector3D& normal)
{
	m_normal = normal;
}

const MVector3D& CullingPlane::getCenter() const
{
	return m_center;
}

const MVector3D& CullingPlane::getNormal() const
{
	return m_normal;
}

bool CullingPlane::isOutside(BoundingSphere* pBSphere)
//bool CullingPlane::isOutside(BoundingVolume* pBV)
{
	float dist = distance(pBSphere);
	if(dist < -pBSphere->getRadius())
		return false;
	
	return true;
}

bool CullingPlane::isOutside(MVector3D& point)
{
	float dist = distance(point);
	if(dist < 0)
		return false;

	return true;
}

float CullingPlane::distance(BoundingBox *pBBox)
{
	//not implemented - always return -1.0 which indicates that there will be no collision
	return -1.0f;
}

float CullingPlane::distance(MVector3D& point)
{
	float dist, planeDistance;
	//MVector3D
	planeDistance = m_normal.DotProduct(m_center);
	dist = m_normal.DotProduct(point) + abs(planeDistance);

	return dist;
}

float CullingPlane::distance(BoundingSphere *pBSphere)
{
	float distance, planeDistance;
	MVector3D sphereCenter = pBSphere->getCenter();

	planeDistance = m_normal.DotProduct(m_center);
	distance = m_normal.DotProduct(sphereCenter) + abs(planeDistance);

	return distance;
}
/*
bool CullingPlane::isOutside(BoundingBox* pBBox)
{
	return true;
}*/

ViewFrustum::ViewFrustum(MVector3D pos, MVector3D target, MVector3D up, float& fNearPlane,  float& fFov, float& fFarPlane, float fRatio)
{
	m_fFOV = fFov;
	m_fRatio = fRatio;
	m_fNearPlaneDist = fNearPlane;
	m_fFarPlaneDist = fFarPlane;

	m_pPlanes = new CullingPlane[NUM_PLANES];

	invalidateFrustum(pos, target, up, fNearPlane, fFov, fFarPlane, fRatio);
}

ViewFrustum::~ViewFrustum()
{
	//for(int i = 0; i < NUM_PLANES; ++i)
	//	delete m_pPlanes[i];

	delete[] m_pPlanes;
}

void ViewFrustum::invalidateFrustum(MVector3D pos, MVector3D target, MVector3D up, float& fNearPlane,  float& fFov, float& fFarPlane, float& fRatio)
{
	MVector3D negLook, aux, normal, rightCenter, leftCenter, bottomCenter, topCenter;

	m_camPos = pos;
	//vector opposite to look vector
	negLook = pos - target;
	negLook.Normalize();
	m_camViewRay = -negLook;

	//size (right) vector
	m_camRight = up.CrossProduct(negLook);
	m_camRight.Normalize();

	//real up vector
	m_camUp = negLook.CrossProduct(m_camRight);
	m_camUp.Normalize();

	//near plane calculations
	m_fNearPlaneHeight = 2 * tan(fFov/2) * fNearPlane;
	m_fNearPlaneWidth = m_fNearPlaneHeight * fRatio;

	//far plane calculations
	m_fFarPlaneHeight = 2* tan(fFov/2) * fFarPlane;
	m_fFarPlaneWidth = m_fFarPlaneHeight * fRatio;

	
	 float farVerticalTrans, farHorizonTrans, nearVerticalTrans, nearHorizonTrans;
	 MVector3D farCenter, nearCenter, one;
	/*MVector3D fFTL, fFTR, fFBL, fFBR;
	MVector3D fNTL, fNTR, fNBL, fNBR;*/
	 one = MVector3D(1.0, 1.0, 1.0);

	farVerticalTrans = abs((up * (m_fFarPlaneHeight/2)).DotProduct(one));
	farHorizonTrans  = abs((m_camRight * (m_fFarPlaneWidth/2)).DotProduct(one));

	farCenter = m_camPos + m_camViewRay*m_fFarPlaneDist;

	//far plane vertices
	/*fFTL = farCenter + farVerticalTrans - farHorizonTrans;
	fFTR = farCenter + farVerticalTrans + farHorizonTrans;
	fFBL = farCenter - farVerticalTrans - farHorizonTrans;
	fFBR = farCenter - farVerticalTrans + farHorizonTrans;*/

	nearVerticalTrans = abs((m_camUp * (m_fNearPlaneHeight/2)).DotProduct(one));
	nearHorizonTrans  = abs((m_camRight * (m_fNearPlaneWidth/2)).DotProduct(one));

	nearCenter = m_camPos + m_camViewRay*m_fNearPlaneDist;

	//near plane vertices
	/*fNTL = nearCenter + nearVerticalTrans - nearHorizonTrans;
	fNTR = nearCenter + nearVerticalTrans + nearHorizonTrans;
	fNBL = nearCenter - nearVerticalTrans - nearHorizonTrans;
	fNBR = nearCenter - nearVerticalTrans + nearHorizonTrans;*/


	//faster approach
	//setting near plane
	m_pPlanes[NEAR_PLANE].setNormal(-negLook);
	m_pPlanes[NEAR_PLANE].setCenter(nearCenter);

	//setting far plane
	m_pPlanes[FAR_PLANE].setNormal(negLook);
	m_pPlanes[FAR_PLANE].setCenter(farCenter);

	//setting left plane
	leftCenter = nearCenter - m_camRight * nearHorizonTrans;
	aux = leftCenter - pos;
	aux.Normalize();
	normal = aux.CrossProduct(m_camUp) ;
	normal.Normalize();
	m_pPlanes[LEFT_PLANE].setNormal(normal);
	m_pPlanes[LEFT_PLANE].setCenter(leftCenter);

	//setting right plane
	rightCenter = nearCenter + m_camRight * nearHorizonTrans;
	aux = rightCenter - pos;
	aux.Normalize();
	normal = m_camUp.CrossProduct(aux);
	normal.Normalize();
	m_pPlanes[RIGHT_PLANE].setNormal(normal);
	m_pPlanes[RIGHT_PLANE].setCenter(rightCenter);

	//setting top plane
	topCenter = nearCenter + m_camUp * nearVerticalTrans;
	aux = topCenter - pos;
	aux.Normalize();
	normal = aux.CrossProduct(m_camRight);
	normal.Normalize();
	m_pPlanes[TOP_PLANE].setNormal(normal);
	m_pPlanes[TOP_PLANE].setCenter(topCenter);

	//setting bottom plane
	bottomCenter = nearCenter - m_camUp * nearVerticalTrans;
	aux = bottomCenter - pos;
	aux.Normalize();
	normal = m_camRight.CrossProduct(aux);
	normal.Normalize();
	m_pPlanes[BOTTOM_PLANE].setNormal(normal);
	m_pPlanes[BOTTOM_PLANE].setCenter(bottomCenter);
}

FRU_COLL_STAT ViewFrustum::isInFrustum(/*BoundingVolume*/BoundingSphere* pBV)
{
	for(short i = 0; i < NUM_PLANES; ++i)
	{
		if(m_pPlanes[i].isOutside(pBV))
			return INSIDE;
	}

	return OUTSIDE;
}

FRU_COLL_STAT ViewFrustum::isInFrustum(BoundingBox *pBV)
{
	int out, in;

	for(short i = 0; i < NUM_PLANES; ++i)
	{
		in = out = 0;

		for(short j = 0; j < 8; ++j)
		{
			MVector3D* corners = const_cast<MVector3D*>(pBV->getCorners());
			if(m_pPlanes[i].isOutside(corners[j]))
				++in;
			else
				++out;
		}
		if(!in)
			return OUTSIDE;
		if(out)
			INTERSECTING;
	}

	return INSIDE;
}

//EOF

